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Preface 

This  report  has  two  purposes.  The  first  purpose  is  to 
satisfy  the  thesis  requirement  for  AFIT.  The  second  purpose 
is  to  investigate  the  feasibility  of  adapting  an  extended 
Kalman  filter  to  the  real-time  satellite- to-satellite 
tracking  problem. 
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I hope  they  find  this  thesis  useful. 

I would  like  to  thank  Capt.  J.  Gary  Reid  for  his  formu- 
lation of  the  extended  Kalman  filter  and  his  assistance  on 
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Goldsmith  for  her  support  with  the  PDP-11/45  minicomputer. 
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Miller,  for  the  latitude  to  complete  this  project. 
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for  her  understanding  and  patience  during  the  preparation 
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Abstract 

[ 

^The  feasibility  of  adapting  an  extended  Kalman  filter 
to  the  real-time  satellite-to-satellite  tracking  problem  is 
investigated.  A filter  using  linear  propagation  and  non- 
linear measurements  is  formulated.  This  filter  accepts 
tracker  measurements  to  provide  the  state  estimates  (posi- 
tion, velocity,  and  acceleration)  of  the  tracked  satellite 
in  the  local  inertial  reference  frame. 

Six  different  filters  are  programmed  in  American 
National  Standards  Institute  (ANSI)  FORTRAN.  These  filters 
employ  various  covariance  update  techniques  and  methods  of 
processing  the  measurement  vector.  The  programs  are  com- 
pared for  computer  program  storage  requirements,  program 
execution  speeds,  and  computational  precision. 

The  computer  program  storage  requirements  are  deter- 
mined by  utilizing  the  FORTRAN  IV-PLUS  compiler  on  a PDP- 
11/45  minicomputer.  The  program  execution  speeds  are  found 
by  timing  on  the  PDP-11/45  the  filter’s  reaction  to  a 
typical  rendezvous  between  two  space  vehicles.  The  computer 
precision  of  each  filter  is  determined  by  comparing  the 
state  estimates  obtained  from  the  60-bit  word  CDC-6600 
computer  with  those  from  the  32-bit  word  PDP-11/45  mini- 
computer. A final  filter  is  selected  as  best  for  the  real- 
time satellite-to-satellite  tracking  problem. 
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INVESTIGATION  OF  REAL-TIME 
SATELLITE-TO- SATELLITE  TRACKING 
USING  EXTENDED  KALMAN  FILTERS 

I . Introduction 
Statement  of  the  Problem 

There  are  many  situations  where  it  is  necessary  to 
track  one  accelerating  satellite  from  another.  A space 
rendezvous  is  one  such  example.  The  tracking  information 
that  is  required  for  this  maneuver  must  be  real-time  (cur- 
rent) and  highly  accurate. 

This  thesis  studies  the  feasibility  of  adapting  an 
extended  Kalman  filter  to  the  problem  of  satellite- to- 
satellite  tracking.  The  problem  of  providing  timely  and 
accurate  tracking  infonnation  is  solved  in  this  thesis  by 
determining  the  state  estimates  (position,  velocity,  and 
acceleration)  of  the  tracked  satellite  relative  to  the 
tracking  satellite.  This  is  accomplished  by  using  an 
extended  Kalman  filter  of  linear  propagation  and  nonlinear 
measurements  in  a local  inertial  reference  frame. 

Scope/Approach 

The  problem  is  solved  by  combining  a set  of  nonlinear 
Kalman  filter  equations,  a collection  of  linear  Kalman  fil- 
ter update  mechanizations,  a definition  of  real-time,  and  a 
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typical  rendezvous  scenario.  The  definition  of  real-time 
is  being  able  to  process  the  measurements  (R,  AZ,  and  EL) 
from  a tracker  and  produce  the  state  estimates  (S  , V , A , 

3C  X X 

S , V A , S , V , and  A ) while  allowing  a time  margin 
y y y z z z 

for  their  use  before  the  next  measurements  arrive.  For  this 
thesis,  the  tracker  supplies  measurements  once  every  tenth 
of  a second.  The  typical  rendezvous  scenario  is  the  meeting 
of  two  space  vehicles  in  low  circular  orbit. 

The  determination  of  the  state  estimates  is  accom- 
plished by  programming  and  analyzing  six  extended  Kalman 
filters.  The  first  five  filters  use  modifications  of 
i Bierman's  covariance  update  mechanizations  of  the  linear 

state/linear  measurement  Kalman  filter  (Ref  2).  The  modi- 
fications to  these  mechanizations  are  documented  by  the 
i software  design  technique  of  Structured  Design.  This  tech- 

nique will  facilitate  future  changes  to  the  programs. 

The  first  five  filters  employ  different  covariance 
update  techniques  and  process  the  measurement  vector  (R,  AZ, 
and  EL)  one  component  at  a time.  The  first  filter  is  the 
conventional  Kalman  filter.  The  second  filter  is  the  stabi- 
lized (Joseph)  Kalman  filter.  The  third  filter  is  the 
modified  Potter  filter.  The  fourth  filter  is  the  modified 
U-D  filter.  The  fifth  filter  is  the  modified  upper  trian- 
gular (Carlson  filter).  These  first  five  filters  are  opti- 
mized for  storage  and  computation  time.  The  sixth  filter 
is  a standard  Kalman  filter,  but  the  measurement  vector  is 
processed  all  at  once.  The  program  for  filter  six  is  also 
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documented  using  Structured  Design.  The  sixth  filter  is 
used  as  a reference  for  the  first  five  filters  and  is  con- 
sidered the  worst  case  from  a computational  load  viewpoint. 
There  is  no  attempt  to  optimize  storage  or  computation  time. 
Its  filter  equations  are  programmed  in  the  pure  matrix- 
vector  form  using  general  matrix  subroutines  (Ref  6 : 65-66). 

The  filter  algorithms  are  programmed  in  American 
National  Standards  Institute  (ANSI)  FORTRAN,  as  described 
in  X. 3-1966.  This  is  done  for  easy  transport  of  the  source 
decks  between  computers.  The  source  code  for  each  filter 
algorithm  was  developed  on  a CDC-6600  computer.  After  the 
completion  of  debugging,  the  source  codes  (card  decks)  were 
transported  to  a PDP-11/45  computer.  The  programs  were 
then  executed  on  the  minicomputer . 

The  six  filters  are  compared  for  computer  program 
storage  requirements,  program  execution  speeds,  and  compu- 
tational precision.  A final  filter  is  selected  as  best  for 
providing  real-time  and  accurate  satellite-to-satellite 
tracking  information. 

Contributions/Products 

This  thesis  makes  several  contributions  and  generates 
various  products  for  future  use.  The  first  contribution  is 
the  determination  of  a set  of  three  extended  Kalman  filters 
that  satisfy  the  definition  of  real-time.  Each  of  these 
filters  is  capable  of  solving  the  satellite- to-satellite 
tracking  problem.  An  interesting  feature  of  these  filters 
is  the  exploitation  of  the  most  current  state  estimates  to 
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i compute  the  measurement  equation  during  processing  of  the 

measurement  vector.  The  second  contribution  is  the  optimi- 
zation of  the  filters  for  storage  and  computation  time. 

Significant  savings  in  memory  locations  and  execution  speed 
are  obtained. 

The  third  contribution  is  a comparison  of  the  test 

i 

I filters  for  storage,  execution  time,  and  computational 

I precision.  Based  upon  these  comparisons  a best  filter  is 

I 

I selected  for  the  real-time  satellite-to-satellite  tracking 

I problem.  The  useful  products  that  remain  from  this  thesis 

( 

I are  the  documentations  of  the  test  filters.  These  consist 

^ of  the  filter  program  listings,  structure  diagrams  for  each 

i ' 

program,  the  nonlinear  equations,  and  the  linear  covariance  ' 

I 

( update  mechanizations.  This  documentation  facilitates  ease  , i 

i 1 

of  modification  for  alternate  target  acceleration  models  i 

(Ref  5 ) or  alternate  measurement  equations  (Ref  4 ) . | 
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II.  The  Extended  Kalman  Filter 


Introduction 

The  extended  Kalman  filter  can  be  a powerful  and  prac- 
tical approach  to  estimating  the  state  estimates  of  an 
accelerating  space  vehicle  given  the  range  and  tracking 
measurements  (Ref  9 ) . Since  this  problem  is  inherently  a 
nonlinear  estimation  problem,  the  extended  Kalman  filter 
has  been  found  to  be  a useful  compromise  between  estimation 
accuracy  and  ease  of  computation  because  of  its  recursive 
nature. 

Description  of  the  Extended  Kalman  Filter  Application 

The  physical  problem  is  how  to  determine  the  states 
(position,  velocity,  and  acceleration)  of  an  approaching 
satellite.  This  problem  is  solved  by  using  an  extended 
Kalman  filter  with  linear  propagation  and  nonlinear  measure- 
ments in  a local  inertial  coordinate  frame.  The  filter 
accepts  three  observed  measurements  from  the  tracker.  These 
measurements  are  range  (R) , azimuth  (AZ) , and  elevation  (EL) . 
The  filter  then  produces  the  state  estimates  (S  , V , A , 

Sy,  Vy,  Ay,  S^,  V^,  and  A^) . The  notations  S^,  V^,  and  A^ 
represent  the  relative  position,  velocity,  and  acceleration 
between  the  two  satellites  in  the  x direction,  respectively. 
The  extended  Kalman  filter  application  for  this  problem  is 
shown  in  Fig.  1 (Ref  6:A1). 

The  tracker  uses  a local  inertial  reference  frame  that 
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is  established  upon  initial  satellite  acquisition.  The  x 
axis  is  aligned  in  the  initial  direction  to  the  tracked 
vehicle.  The  y and  z axes  are  selected  with  reference  to 
the  tracker  orientation.  The  selection  of  this  reference 
frame  forces  the  initial  R to  be  on  the  x axis  and  the 
initial  AZ  and  EL  to  be  zero.  Figure  2 shows  the  local 
inertial  coordinate  frame  with  its  origin  at  the  tracker. 

Extended  Kalman  Filter  Equations 

The  extended  Kalman  filter  equations  are  stated  for 
this  thesis.  I’he  pertinent  equations  are  divided  into 
three  categories:  the  system  equations,  the  propagation 
equations,  and  the  measurement  equations  (Ref  10). 

Since  a local  inertial  coordinate  system  is  assumed 
for  the  state  estimates,  the  state  equation  in  the  time 
invariant  discrete  form  is 

x(t^^^)  =•  $x(t^)  + w(tj^)  (1) 

where  x is  the  system  state  vector  (9  elements)  consisting 
of  the  relative  position,  velocity,  and  acceleration  along 
each  of  the  three  inertial  coordinate  directions . $ is  the 

system  state  transition  matrix  (9X9).  The  notation  (9  X 
9)  specifies  the  size  of  a matrix  where  the  first  number 
equals  the  number  of  rows  and  the  second  number  equals  the 
number  of  columns.  The  zero  mean  Gaussian  white  noise 
( vector,  w(tj^) , has  nine  elements.  The  discrete  time 
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measurement  equation  Is 


2(tj^)  - h(x(t^),t^)  + v(tj^)  (2) 

where  i is  the  measurement  vector  (3  elements)  consisting 
of  range,  azimuth,  and  elevation  measurements.  The  zero 
mean  Gaussian  white  noise  vector,  vCt^) , has  three  elements. 
The  filter  propagation  equations  are  then 

x(tj^")  “ ix(tj^_^'*’)  (3) 


and 


+ Q 


(4) 


where  ♦ is  the  system  state  transition  matrix  (9  X 9)  over 
the  interval  tj^~)  . P is  the  covariance  matrix  (9  X 

9)  of  estimation  error.  Q is  a 9 X 9 diagonal  matrix  whose 
diagonal  elements  consist  of  the  noise  strength  of  each 
corresponding  element  of  the  zero  mean  Gaussian  noise  pro- 
cess w(tj^).  The  update  or  measurement  equations  are 


K(x(t^").tJ  - P(t^")H'^(i(t^‘).tJ|H(x(t^"),ti  ) 


P(ti")H'^(x(ti").tj)  +R(tj^)j-‘  (5) 
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Fig.  3 Extended  Kalman  Filter  (Ref  6=63) 


xCtj^'*’)  - x(tj^’)+K(x(tj^“)  ,tj^)  z(tj^)-h(x(tj^‘)  , tj^)  j (6) 


PCt^"*")  - P(t^")-K(x(tj^")  ,t^)H(x(t^")  ,t^)P(tj^”) 


K is  the  Kalman  gain  matrix  (9X3).  H is  the  measurement 
matrix  (3X9).  R is  the  measurement  noise  covariance  mdtiix 
(3X3).  As  Fig.  3 shows,  the  recursive  nature  of  the 
extended  Kalman  filter  is  obvious  (Ref  6 :63). 


Reduction  of  Propagation  Computations 


Since  the  comparison  of  filter  execution  speeds  is  one 
of  the  objectives  of  this  thesis,  methods  of  reducing  the 
computational  workload  were  investigated.  It  was  noticed 
that  the  number  of  computations  involving  the  P and  ♦ 


matrices  could  be  reduced. 


Equation  1 Is  of  the  3 X 


3 block  decoupled  from 


where 


4> 


exponential 


0 

( 0 
0 


(8) 


(9) 


The  acceleration  correlation  (bandwidth)  parameter  Is  y- 
The  duration  of  time  between  measurement  updates  Is  A.  Note 
that  these  equations  are  decoupled  around  the  x,  y,  and  z 
axes . 

Solving  for  ♦ gives 
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Q is  a white  driving  noise  submatrix  approximation  for  Q. 


(14) 


where 


(15) 


The  elements  of  Q are  selected  by  the  design  engineer 
during  tuning  to  optimize  the  performance  of  the  filter.  A 
more  complex  approximation  of  the  discrete  driving  noise 
term  could  be  used  if  desired  (Ref  1 ) . This  modification 
can  easily  be  made  to  the  existing  filter  programs.  The 
weighting  function  i5(j,k)  is  one  if  j equals  k and  zero 
if  J is  not  equal  to  k. 

Measurement  Computations 

Since  the  measurement  equations  are  nonlinear,  the 
estimated  measurements,  h(x(t^~) , , depend  upon  the 

current  a priori  state  estimates  This  can  be  seen  in  the 
equations  for  the  predicted  measurements. 

ZP^(tj^")  - ^x^  (tj_")  *+x^  ^^i~^  *'^*7 


1 
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ZP^(ti")  - arctan(x^  (tj^‘)/x^  (17) 

“ arctan  (u)  (18) 

2Pj(t^")  » arcsin(x^ (t^")/ZP^ (t^‘))  (19) 

• arcsin  (v)  (20) 


The  linearized  H matrix  also  depends  upon  the  current  state 
estimates.  Using  the  conventions  established  in  Equations 
16  through  20, 


‘h 

1 1 

0 

0 

H 

I *> 

0 

0 

H 

1 7 

0 

1 

o 

H - 

H 

2 1 

0 

0 

H 

2 4 

0 

0 

0 

0 

0 

H 

3 1 

0 

0 

H 

3 4 

0 

0 

H 

3 7 

0 

0 

where 


i 


(21) 


) 


H 

1 1 

- (t^")/ZP^  (tj^‘) 

(22) 

- -u/((l+u*)x^(tf )) 

(23) 

- (-V/  ^1-v*) (Xj(t^“)/ZP^(t^")*) 

(24) 

- x^(Ci-)/ZP^(4“) 


(25) 
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H 

2 % 

- l/((l+u*)x^(t^‘)) 

(26) 

[ ” 

1 i h 

\ 

- (-V/  (x^(t^’)/ZP^(t^')*) 

(27) 

f 

! * ^ 

I 

- x^(t^~)/ZP^ (t^') 

(28) 

i 

-[(1/  (l/ZP^(t.'))]-[vx^(t^')/ZP^(t^')*] 

(29) 

The  satellite- to-satellite  tracking  problem  is  solved  by 
implementing  the  developed  extended  Kalman  filter  equations 
5,  6,  7,  11,  13,  16,  18,  20,  and  21  into  a recursive 


computer  program.  The  program  accepts  measurements  from 
the  tracker  and  produces  the  desired  state  estimates  of 
the  tracked  satellite. 


III.  Implementation  and  Testing 


To  test  the  extended  Kalman  filter  equations  of 
Chapter  II,  a typical  rendezvous  is  proposed  and  six  test 
filters  are  investigated.  The  filters  are  analyzed  for 
computer  program  storage  requirements,  program  execution 
speeds,  and  computational  precision. 

Typical  Rendezvous 

The  typical  rendezvous  is  assumed  to  be  the  tracker's 
observations  of  a tracked  satellite  for  ten  seconds  follow- 
ing initial  acquisition.  The  tracker  is  mounted  on  a 
vehicle  in  low  circular  orbit  (193  X 193  km) . The  notation 
(193  X 193  km)  specifies  an  orbit  with  perigee  and  apogee 
at  193  km  above  the  earth's  surface,  respectively.  The 
tracked  vehicle  is  in  a higher  elliptical  orbit  (193  X 820 
km) . Both  satellite  orbits  are  in  nearly  the  same  plane 
and  rendezvous  occurs  at  perigee  for  both  satellites . The 
tracker  begins  providing  information  when  the  scanned 
satellite  comes  within  55,000  meters.  From  the  point  of 
initial  acquisition,  tracking  information  is  provided  ten 
times  per  second  for  ten  seconds.  A sampling  of  this  infor- 
mation is  shown  in  Appendix  A. 

Test  Filters 

The  six  filter  implementations  are  variations  of  covar- 
iance update  techniques  and  methods  of  processing  the 
measurement  (observation)  vector.  The  first  five  filters 
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use  adaptations  of  Bierman's  mechanir^atlons  for  the  non- 
linear system.  They  process  the  measurement  vector  one 
component  at  a time.  The  first  filter  is  the  conventional 
Kalman  filter  with  the  covariance  update  equation  of 

P(t^'*')  - P(t^')  - Ka'^P(tj^‘)  (30) 

- »r 

where  K is  a column  from  K and  a is  a row  from  H.  The 
second  filter  is  the  stabilized  Kalman  (Joseph)  filter  with 
the  covariance  update  equation  of 

^(t^"^)  - (I-Ka'^)P(tj^")  (I-Ka’^')’’’  + KtR*^  (31) 

where  I is  the  identity  matrix  and  r is  an  element  from  the 
diagonal  of  R.  The  third  filter  is  a modified  Potter  filter 
with  the  covariance  update  equation  of 

P(t^^‘^)  - S(t^‘*')S(t^'*')'^  (32) 

where  S is  obtained  from  Bierman's  Potter  covariance  update 
mechanization.  The  fourth  filter  is  a modified  U-D  filter 
with  the  covariance  update  equation  of 

- U(tj^'*')5(tj^'*‘)U(tj^'’‘)’^  (33) 

where  U and  D are  obtained  from  Bierman's  U-D  covariance 
update  mechanization.  .The  fifth  filter  is  a modified  upper 
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triangular  (Carlson)  filter  with  the  covariance  update 
equation  of 


- U(t^‘^)U(t^'^)''^  (34) 

where  U is  obtained  from  Bierman's  upper  triangular  covar- 
iance update  mechanization.  The  sixth  filter  is  the 
standard  Kalman  filter.  It  processes  all  the  observations 
(z  vector)  at  the  same  time  and  uses  the  covariance  update 
equation  of 


- P(tj^")  - i^P(t^")  (35) 

Filters  three,  four,  and  five  are  modified  square  root  fil- 
ters and  should  produce  state  estimates  with  better  accuracy 
than  filters  one,  two,  or  six.  For  faster  execution  at  the 
expense  of  accuracy,  these  modified  square  root  filters  use 
the  Matrix  RSS  method  to  compute  P(b^^j")  and  then  generate 
the  square  root  covariance  matrices  (S(tj^^j~),  U-D(tj^^i~), 
and  U(tj^^^")).  Therefore  they  possess  only  the  same  numer- 
ical precision  during  propagation  as  filters  one,  two,  and 
six  (Ref  7 :Ch  7,16).  The  sixth  filter  represents  the 
typical  filter  implementation  and  is  used  as  reference  for 
storage,  execution  time,  and  computational  comparisons  with 
the  first  five  filters. 

No  attempt  was  made  to  tune  any  of  the  filters.  Tuning 
is  defined  as  the  process  of  optimizing  the  parameters  of 
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the  Q and  R matrices  for  optimal  filter  accuracy.  The 
initial  conditions  for  Q,  and  R are  shown  in 

Appendix  B. 


Software  Design 

The  six  extended  Kalman  filter  algorithms  are  developed 
with  the  software  design  technique  of  Structured  Design . 
’’Structured  design  is  the  art  of  designing  the  components 
of  a system  in  the  best  possible  way  (Ref  11:7)."  This 
technique  was  chosen  because  it  tends  to  produce  programs 
that  have  low  development  costs,  low  maintenance  costs,  and 
low  modification  costs  (Ref  11:254).  These  low  costs  are 
achieved  by  designing  computer  programs  that  have  low  coup- 
ling between  modules  and  high  cohesion  within  modules. 
Coupling  is  a measure  of  the  strength  of  interconnection 
between  software  modules  (Ref  11:116).  Cohesion  is  a 
measure  of  the  strength  of  internal  bonding  of  a software 
module  (Ref  11:143).  This  technique  is  used  because  it 
produces  more  flexible  and  complete  documentation  than  flow 
charts,  which  are  limited  to  control  coupling  only. 

The  major  components  of  this  technique  are  the  data 
flow  graph  and  the  structure  diagram.  The  data  flow  graph 
identifies  the  primary  flow  of  data  through  the  system. 

The  data  flow  graph  for  all  six  filters  is  shown  in  Fig.  4. 
This  figure  shows  the  measurements  as  the  high  level  inputs 
and  the  state  estimates  as  the  high  level  outputs.  The 
primary  processing  functions  are  the  update  and  propagate 

portions  of  the  extended  Kalman  filter. 
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Fig.  4 Data  Flow  Diagram  for  Filters  One  Through  Six 


The  structure  diagram  shows  the  factoring  of  the  data 
flow  graph  into  its  corresponding  afferent  (READ) , efferent 
(WRITE) , and  central  transform  (PROPAGATE  and  UPDATE) 
modules.  The  structure  diagrams  for  the  first  five  filters 
are  shown  in  Figures  5-9.  Figure  10  illustrates  the  struc- 
ture diagram  for  the  sixth  filter.  By  comparing  the 
UPDATE  modules  from  Figures  5-9  and  10,  the  difference  in 
processing  the  measurement  vector  becomes  apparent.  Since 
each  filter  program  will  probably  be  implemented  as  a 
separate  subroutine,  some  design  license  is  used  to  name 
the  submodules  of  the  filter  program.  Submodules  are  named 
for  fxmctions  performed  rather  than  using  subroutine  call 
names . 

The  first  five  filter  programs  can  be  criticized  for 
having  modules  with  higher  coupling  between  modules  and 
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Fig.  6 Structure  Diagram  for  Filter  Two 


Control 


Fig,  7 Structure  Diagram  for  Filter  Three 
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Structure  Diagram  for  Filter  Six 
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lower  cohesion  within  modules  than  filter  six.  The  UPDATE 
modules  in  filters  one  through  five  suffer  from  control 
coupling  between  modules  (Ref  11:125).  The  subordinate 
modules  of  the  UPDATE  module  must  know  which  element  of  the 
measurement  vector  (i)  is  being  processed.  This  control 
coupling  along  with  the  normal  data  coupling  is  essential 
for  the  UPDATE  modules  to  perform  their  functions.  Modules 
that  perform  more  than  one  function  also  have  lower  cohesion 
within  modules  (Ref  11:171-172).  Ideally  a module  should 
perform  only  one  function,  but  when  more  than  one  are  per- 
formed then  the  module  has  low  logical  cohesion  (Refll:153- 
15A) . The  higher  coupling  between  modules  and  lower  cohe- 
sion within  modules  in  filters  one  through  five  should  make 
development,  maintentx,  and  modification  costs  higher  for 
these  filters  than  filter  six. 

The  data  flow  diagram  and  the  structure  diagrams  along 
with  the  actual  program  listing  constitute  the  major  portion 
of  the  documentation  for  the  extended  Kalman  filter 
algorithms.  This  documentation  illustrates  how  the  non- 
linear system  equations  and  the  modified  linear  mechaniza- 
tions are  combined  into  useful  programs.  Because  this 
documentation  is  so  flexible  and  complete,  it  allows  easy 
program  modification.  For  example,  it  was  noticed  that  the 
most  current  state  estimates  in  filters  one  through  five 
could  be  exploited  in  the  measurement  equations.  This 
change  was  made  to  the  programs  and  documented  on  the 
structure  diagrams.  This  improvement  to  the  filters  might 


not  have  been  seen  if  flowcharts  were  used  as  documentation. 


Programming  Philosophy 

Since  this  thesis  investigates  the  application  of  an 
extended  Kalman  filter  in  real-time,  the  reduction  of  filter 
execution  speeds  is  a major  consideration.  The  programming 
philosophy  minimizing  execution  time  without  incurring 
excessive  storage  requirements  was  applied  to  filters  one 
through  five.  Filter  six  is  used  as  a reference  because  it 
is  a common  implementation  of  the  extended  Kalman  filter 
using  general  purpose  matrix  subroutines.  This  comparison 
should  indicate  if  the  optimization  effort  for  filters  one 
through  five  was  worth  the  reduction  in  execution  times . 

The  implementation  of  equations  11  and  13  saves  consid- 
erable computation  time  during  the  propagation  of  x and  P. 
Further  time  reductions  are  achieved  by  eliminating  unneces- 
sary computations.  The  reductions  are  of  two  classes: 
sparse  matrix  computations  and  redundant  computations  of 
the  symmetric  P matrix. 

The  realization  that  several  matrices  (<f,  R,  H,  and  Q) 
are  sparse  and  the  subsequent  reduction  of  noncontributing 
computations  greatly  diminished  filter  execution  speeds, 
i Along  with  the  decrease  in  execution  times,  a reduction  in 

storage  requirements  is  also  achieved.  The  matrix  is 
further  reduced  from  a 3 X 3 matrix  to  4 constants  (PHI12, 
PHI13,  PHI23,  and  PHI33).  The  R matrix  is  decreased  from 
a 3 X 3 matrix  to  a 3 element  vector  (R(l),  R(2),  and  R(3)). 
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The  Q matrix  is  diminished  from  a 3 X 3 matrix  to  3 con-  | 

stants  (QX,  QV,  and  QA) . The  H matrix  is  reduced  from  a 1 

T \ 

3X9  matrix  to  a 9 element  vector  (a  ) . Additional  I 

unnecessary  computations  during  filter  update  are  eliminated  j 

because  as  Eq.  21  shows  only  the  first,  fourth,  and  seventh 

elements  of  any  row  ever  contain  nonzero  entries . 

The  second  class  of  computational  reductions  are 
achieved  with  the  P matrix.  Since  P is  a symmetric  matrix, 
it  is  unnecessary  to  recompute  an  element  P^j  if  Pj^  has 
already  been  computed. 

Because  all  the  information  contained  in  the  full 
matrix  can  be  found  in  the  upper  triangular  portion,  only 
that  portion  is  computed  and  used  during  filter  propagation 
and  update. 

Comparison  Procedure 

The  extended  Kalman  filter  programs  are  programmed  in 
ANSI  standard  FORTRAN  for  ease  of  transport  between  the 
CDC-6600,  the  PDP-11/45  and  an  undetermined  application 
computer.  The  FORTRAN  programs  for  the  six  test  filters  are 
provided  in  Appendix  C.  The  programs  were  built  on  the  CDC- 
6600  machine  because  it  was  readily  available  and  it  pos- 
sessed extensive  program  debugging  capabilities.  After  the 
programs  were  debugged,  they  were  transferred  to  the  PDP- 
11/45  in  the  form  of  card  decks.  The  PDP-11/45  was  selected 
for  program  execution  because  it  resembles  the  typical 
space-bome  computer  that  is  capable  of  32-bit  floating 
point  arithmetic  operations.  The  source  decks  were  then 
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executed  using  the  RSX-llM  operating  system.  This  operating 
system  normally  operates  in  a multiprogramming  mode,  but  it 
was  assiimed  to  provide  good  execution  speed  approximations 
because  the  programs  were  individually  executed. 

The  filter  programs  were  compiled  with  the  FORTRAN 
IV-PLUS  compiler  on  the  PDP-11/45.  This  compiler  performs 
a number  of  optimizations  on  the  source  code. 

1.  Constant  folding.  Integer  constant  expressions 
are  evaluated  at  compile-time . 

2.  Compile-time  constant  conversion. 

3.  Compile- time  evaluation  of  constant  subscript 
expressions  in  array  calculations. 

4.  Argument- list  merging.  If  two  function  or  sub- 
routine references  have  the  same  arguments,  a 
single  copy  of  the  argument  list  is  generated. 

5.  Branch  instruction  optimizations  for  arithmetic 
and  logical  IF  statements. 

6.  Elimination  of  unreachable  code. 

7.  Recognition  and  replacement  of  common  sub- 
expressions . 

8.  Removal  of  invariant  computations  from  DO 
loops . 

9.  Local  register  assignment.  Frequently  refer- 
enced variables  are  retained  (if  possbile)  in 
registers  to  reduce  the  number  of  load  and 
store  instructions  required. 
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10.  Assignment  of  frequently  used  variables  and 
expressions  to  registers  across  DO  loops. 


(Ref  3 :Ch  4,4). 

The  six  filter  programs  are  tested  with  the  one  hundred 
data  sets  from  the  typical  rendezvous  and  compared  for  com- 
puter program  storage  requirements,  program  execution  speeds 
and  computational  precision.  The  computer  program  storage 
requirements  are  obtained  from  the  Program  Section  Summary. 
This  summary  is  provided  after  each  PDP-11/45  compilation. 
The  program  execution  speed  for  each  filter  is  obtained  by 
subtracting  its  input  and  output  (I/O)  time  from  its  total 
execution  time  and  then  by  dividing  the  difference  by  the 
number  of  iterations  the  filter  accomplished.  Thus 

program  execution  speed  = 


total  execution  time  - I/O  time 
100 


(36) 


An  evaluation  of  the  computational  precision  for  each  filter 
is  obtained  by  comparing  the  state  estimates  from  the  60-bit 
word  CDC-6600  computer  with  the  32-bit  state  estimates  from 
the  PDP-11/45  minicomputer  after  100  iterations.  This  com- 
parison shows  the  effect  of  finite  word  length  on  the 
filter  algorithm.  This  is  typically  called  round-off  error. 
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IV.  Results  and  Conclusion 
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To  select  the  best  filter  for  solving  the  real-time 
satellite- to-satellite  tracking  problem,  the  six  filters 
are  compared  for  computer  program  storage  requirements,  pro- 
gram execution  speeds,  and  computational  precision.  All  the 
filters  performed  as  expected  with  the  exception  of  filter 
three.  Filter  three  continues  to  produce  state  estimates 
that  vary  greatly  from  the  other  filters.  The  reason  for 
this  condition  is  still  undetermined.  Therefore,  filter 
three  is  not  seriously  considered  for  the  best  filter,  but 
its  comparisons  are  performed  and  documented. 


Computer  Program  Storage  Requirements 

The  computer  program  storage  requirement  for  each  fil- 
ter is  obtained  from  the  PDP-11/45  Program  Section  Summary  ^ 

(Ref  3:Ch  1,8).  Since  the  PDP-11/45  assembly  language 
includes  both  single  (16-bits)  and  double  (32-bits)  word 

instructions,  the  standard  unit  of  storage  is  the  byte  (8-  i 

bits).  Table  I shows  the  computer  program  storage  require- 
ment for  each  filter  in  bytes.  For  the  optimized  filters 

1-5,  filter  1 is  the  smallest  and  filter  4 is  the  largest.  j 

1 

Filter  6 proves  to  be  the  worst  case  - as  was  expected.  It 
requires  from  87  to  50  percent  additional  storage  over  the 
smallest  and  the  largest  optimized  filter,  respectively. 

This  difference  in  storage  requirements  is  due  to  the  associ- 

I 

ated  matrix  subroutines  of  filter  6.  j 
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Table  I 


Computer  Program  Storage  Requirements 


Filter 

1 

1 

1 ^ 

4 

5 

6 

Bytes 

1763 

1843 

1915 

1874 

2890 

Program  Execution  Speeds 

The  average  program  execution  speed  for  each  filter  is 
obtained  by  utilizing  Eq.  36.  The  total  execution  and  total 
I/O  times  are  determined  by  using  the  SECNDS  instruction  on 
the  PDP-11/45.  This  instruction  provides  the  central  pro- 
cessor unit  (CPU)  time  from  the  moment  it  is  set  until  it 
is  called.  The  total  execution  time  is  found  by  setting  the 
SECNDS  instruction  prior  to  the  main  program  iteration.  The 
instruction  is  then  called  after  100  iterations.  This  total 
execution  time  contains  all  the  times  for  computer  logic, 
indexing  of  variables,  data  transfers,  filter  computations, 
and  I/O  operations.  The  total  I/O  time  is  derived  by 
removing  all  the  filter  computations  from  the  program's 
main  iteration  and  then  running  the  program  again.  This 
removes  all  the  times  for  computer  logic,  indexing  of  vari- 
ables, data  transfers,  and  filter  computations;  but  leaves 
the  time  for  I/O  operations.  These  average  program  execu- 
tion speed  results  appear  in  Table  II.  Actual  times  may 
vary  +15  percent  to  -10  percent  (Ref  8 :App  B,  10). 

As  Table  II  shows,  all  the  optimized  filters  execute 
In  about  0.05  seconds  with  the  exception  of  filters  3 and  5. 


Table  II 

Program  Execution  Speeds 


Filter 

1 

2 

3 

4 

5 

6 

Sec 

.0496 

.0502 

.1011 

.0503 

.1239 

.0763 

! 

i 


Filters  3 and  5 execute  in  0.10  and  0.12  seconds,  respec- 
tively. This  slower  execution  rate  is  due  to  the  need  for 
taking  the  square  root  of  numerous  numbers.  Filters  1,  2, 
4,  and  6 allow  a time  margin  between  measurements  for  use 
of  the  computed  state  estimates.  They  satisfy  the  defini- 
tion of  real-time  application.  Filters  3 and  5 do  not. 
They  are  too  slow  to  keep  up  with  the  tracker.  Filter  6 
does  surprisingly  well  considering  the  number  of  matrix 
subroutine  calls  it  must  accomplish.  Although  slower  than 
filters  1,  2,  and  4,  it  provides  a useful  time  margin 
between  measurements. 


Computational  Precision 

The  comparison  for  computational  precision  is  based  on 
the  normalized  error  (NE)  between  the  final  (100th  itera- 
tion) state  estimates  from  the  60-bit  result  (CDC-6600)  and 
the  32-bit  result  (PDP-11/45).  The  normalized  error  test 
is  defined  as 


NE  - L 
i-i 


(37) 


1 


"i 


J 
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Table  III 


Computational  Precision 


Filter 

1 

2 

3 

4 

5 

6 

N.E. 

.00062 

.00058 

.00002 

.00056  j 

.00058 

.00066 

where  is  the  ith  state  estimate  from  the  60-bit  result. 

The  findings  of  this  test  are  delineated  in  Table  III.  This 
test  is  designed  to  show  round-off  error  trends  due  to 
finite  computer  word  lengths.  The  lack  of  computer  round- 
off error  is  indicated  by  a normalized  error  equal  to  zero. 

Since  filters  3,  4,  and  5 are  square  root  filters, 
their  normalized  errors  should  be  less  than  filters  1,  2, 
and  6.  The  low  normalized  error  for  filter  3 contributes 
to  the  general  lack  of  confidence  in  the  filter.  The  nor- 
malized errors  for  filter  4 and  5 seem  more  reasonable  when 
compared  with  the  other  filters.  Filter  2 seems  to  be  as 
good  as  filter  5 and  much  better  than  filters  1 and  6. 

Filter  6 again  proves  itself  to  be  the  worst  case.  Table 
III  indicates  that  it  suffers  the  most  from  computer  round- 
off error.  This  condition  may  be  aggravated  by  the  observed 
development  of  a nonsymmetric  covariance  (P)  matrix  in  the 
filter  program.  This  problem  will  require  additional  code 
to  fix,  resulting  in  more  storage  requirements  and  slower 
execution  speed. 

Conclusion 


This  study  investigates  the  feasibility  of  adapting  an 


r’i 


i 
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extended  Kalman  filter  to  the  real-time  satellite- to- 
satellite  tracking  problem.  Six  filters  of  linear  propa- 
gation and  nonlinear  measurements  are  tested.  They 
determine  the  state  estimates  (position,  velocity,  and 
acceleration)  of  the  tracked  satellite  relative  to  the 
tracking  satellite  in  a local  inertial  reference  frame.  The 
six  filters  employ  different  covariance  update  techniques 
and  methods  of  processing  the  measurement  vector.  Filters 
one  through  five  exploit  the  most  current  state  estimates 
after  each  scalar  measurement  to  improve  the  filters'  per- 
formance. This  is  not  possible  with  filter  six.  This 
feature  is  illustrated  in  the  structure  diagrams  for  the 
filters.  Three  filters  qualify  for  real-time  application, 
but  filter  four  is  felt  to  give  the  best  overall  performance 
with  respect  to  computer  program  storage  requirements,  pro- 
gram execution  speed,  and  computational  precision. 

The  computer  program  storage  requirements  fell  into 
two  groups.  The  first  group  is  made  up  of  the  first  five 
filters.  These  filters  are  optimized  for  storage  require- 
ments. Matrices  are  reduced  to  a few  constants.  Small 
utility  matrices  and  constants  are  used  when  possible.  The 
second  group  consists  of  filter  six.  It  is  the  worst  case 
reference  with  its  complete  matrices  and  associated  matrix 
subroutines.  A saving  of  between  87  and  50  percent  can  be 
achieved  by  chosing  one  of  the  optimized  filters  over  filter 
six. 

Since  the  application  of  the  extended  Kalman  filter  is 
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to  be  real-time,  program  execution  speed  is  a major  consid- 
eration. The  filter  program  must  be  able  to  keep  up  with 
the  tracker  and  still  allow  time  for  the  use  of  its  state 
estimates.  If  the  tracker  supplies  measurements  every 
tenth  of  a second,  then  filters  three  and  five  are  too  slow 
for  the  real-time  application.  Because  filters  one,  two, 
and  four  operate  at  about  0.05  second,  any  one  of  them  will 
supply  the  state  estimates  in  a timely  manner.  Filter  six 
is  eliminated  because  it  does  not  allow  nearly  as  much  time 
as  filters  one,  two,  and  four. 

Computational  precision  is  the  final  selection  criteria. 
Besides  being  small  and  quick,  the  filter  must  maintain  its 
accuracy.  The  normalized  error  test  indicates  trends  in 
round-off  error  accimulations  due  to  finite  computer  word 
lengths.  The  filter  with  the  closest  value  to  zero  should 
have  the  best  computational  precision.  Excluding  filter 
three,  filter  four  provides  the  most  accurate  state  estimates. 

Therefore,  based  on  a comparison  of  computer  program 
storage  requirements,  program  execution  speeds,  and  compu- 
tational precision,  filter  four  is  the  best  filter  to  solve 
the  real-time  satellite-to-satellite  tracking  problem. 

Other  contributions  are  the  demonstrated  savings  in  storage 
and  execution  time  for  the  optimized  filters  and  the  docu- 
mentation that  remains  for  each  filter  studied. 

Recommendations 

There  are  a number  of  efforts  that  could  continue  the 
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work  begun  by  this  thesis.  The  first  effort  should  be  the 
tuning  of  the  R and  Q matrices  for  optimal  filter  perform- 
ance. This  will  require  considerable  computer  time  to 
match  the  filter  state  estimates  with  the  true  system.  Once 
the  tuning  of  the  filter  is  accomplished,  a Monte  Carlo 
analysis  should  be  performed,  since  this  is  inherently  a 
nonlinear  estimation  problem  (Ref  5 ) . This  will  test 
1 the  filter's  reaction  to  random  noise  sources  that  corrupt 

I the  measurements  and  to  various  trajectory  scenarios. 

! Different  rendezvous  approaches  should  be  investigated 

I to  study  the  effect  on  program  execution  speed  and  computa- 

tional precision.  If  additional  accuracy  is  required  and 
the  time  margin  is  long  enough,  then  the  fast  Matrix  RSS 
propagation  module  could  be  changed  to  either  a House- 
holder's transformation  or  a modified  Gram-Schraidt  module 
(Ref  7:Ch  7,  16). 

Although  this  thesis  addresses  the  feasibility  of 
adapting  an  extended  Kalman  filter  to  the  real-time 
satellite- to-satellite  tracking  problem,  it  has  another  use. 
It  can  serve  as  an  academic  model  for  tracking  any  acceler- 
ating vehicle  from  another  (Ref  4 ).  In  this  respect, 
the  long  term  benefits  of  this  thesis  might  be  the  most 
useful. 
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Appendix  A 


Description  of  Typical  Rendezvous 

This  Is  a representative  time  history  of  the  100  Input 
measurements  for  an  approaching  satellite  In  an  elliptical 
orbit  (193  X 820  km)  closing  on  a satellite  In  low  circular 
orbit  (193  X 193  Kra) . Ranges  (R)  are  In  kilometers.  Angles 
(AZ  and  EL)  are  In  radians.  Measurements  begin  upon  Initial 
acquisition  and  continue  for  ten  seconds.  Initially  the  x 
axis  points  toward  the  tracked  satellite,  thus  making  AZ 
and  EL  zero.  For  this  rendezvous  measurements  of  EL  remain 
below  the  tracker's  measurement  threshold. 


Seconds 

R 

AZ 

EL 

0.0 

54.338 

0.0000 

0.0000 

0.5 

54.255 

-0.0001 

0.0000 

1.0 

54.172 

-0.0001 

0.0000 

1.5 

54.089 

-0.0002 

0.0000 

2.0 

54.006 

-0.0002 

0.0000 

2.5 

53.923 

-0.0003 

0.0000 

3.0 

53.840 

-0.0004 

0.0000 

3.5 

53.757 

-0.0004 

0.0000 

4.0 

53.674 

-0.0005 

0.0000 

4.5 

53.591 

-0.0005 

0.0000 

5.0 

53.508 

-0.0006 

0.0000 

5.5 

53.425 

-0.0007 

0.0000 
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Seconds 


R 


AZ 


EL 


ii 


r 

l 

< 


6.0 

53.341 

-0.0007 

0.0000 

6.5 

53.258 

-0.0008 

0.0000 

7.0 

53.175 

-0.0008 

0.0000 

7.5 

53.092 

-0.0009 

0.0000 

8.0 

53.009 

-0.0009 

0.0000 

8.5 

52.926 

-0.0010 

0.0000 

9.0 

52.842 

-0.0011 

0.0000 

9.5 

52.759 

-0.0011 

0.0000 

10.0 

52.676 

-0.0012 

0.0000 
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Appendix  B 


\ Extended  Kalmnn  Filter  Matrices 

r 

I ; 

The  initial  conditions  for  the  untuned  extended  Kalman 
filters  are 


I 


I 


R - 


54.338  km 
0 km/ sec 

0 km/ sec* 

0 km 

0 km/sec 

0 km/ sec* 

0 km 

0 km/ sec 

0 km/ sec* 


.0001  km* 
0 
0 


0 0 
.00001  rad*  0 

0 .00001  rad* 
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Appendix  C . 1 


Filter  One : Convent ionnl  Kalman  Filter  Program 

Filter  one  represents  the  simplest  form  of  the  extended 
Kalman  filters  that  process  the  measurement  vector  one 
component  at  a time.  It  employs  the  covariance  update  tech- 
nique of 

- f(tj^‘)  - Ka'^P(tj^‘)  (30) 

Bierman  implements  this  equation  (Ref  2. ‘21)  in  his  linear 
Kalman  data  processing  algorithm  (Ref  2:29).  His  FORTRAN 
mechanization  is  modified  in  this  program  for  the  nonlinear 
system  and  is  improved  with  respect  to  storage  requirements 
and  execution  speed.  Propagation  is  accomplished  by  uti- 
lizing Equations  11  and  13 . 

This  program  is  written  in  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documenta- 
tion consists  of  internal  comment  cards,  Fig.  5,  and 
Bierman 'a  equations  and  FORTRAN  mechanization  (Ref  2:28-29). 
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Appendix  C . 2 


Filter  Two : Stabilized  Kalman  Filter  Program 

Filter  two  is  a modification  of  filter  one.  It  also 
processes  the  measurement  vector  one  component  at  a time, 
but  employs  the  covariance  update  technique  of 

P(tj^'*')  •=  (I-Ka'^)P(tj^')(I-Ka'’^)'^  + KrK*^  (31) 

for  better  accuracy  (Ref  2:20).  Bierman  implements  this 
equation  in  his  linear  Kalman  data  processing  algorithm 
(Ref  2:29).  His  FORTRAN  mechanization  is  modified  in  this 
program  for  the  nonlinear  system  and  is  improved  with 
respect  to  storage  requirements  and  execution  speed.  Propa- 
gation is  accomplished  by  utilizing  Equations  11  and  13. 

This  program  is  written  in  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documentation 
consists  of  internal  comment  cards.  Fig.  6,  and  Bierman 's 
equations  and  FORTRAN  mechanization  (Ref  2:28-29). 


54 


* 

♦ 

* 

* 

t» 

A 

K 

♦ 

X 0. 

♦ 

y 

ro 

«- 

w 

e 

•b 

% 

1 

1 

* 

X 

X 

♦ 

»- 

II 

4> 

hJ 

»- 

* 

* 

=> 

M 

♦ 

_i 

0- 

•b 

3 

a 

CVJ 

ro  ^ 

►- 

♦ 

O 

»- 

4> 

O' 

»H  eg  y 

X 

3 

M 

* 

O 

=> 

o 

IH 

M w 

O 

w 

* 

r 

c 

« 

»- 

X 

X X 

a' 

M ^ 

♦ 

o 

a 

a 

•- 

UJ 

UJ 

«- 

Ul 

» 

<c 

z 

• 

«• 

►- 

M # 

> 

«b 

4 

♦ 

o 

^ ro 

« 

«x 

X * 

K. 

ro 

z 

♦ 

15 

X a 

c 

a * 

X 

•k 

fO 

o. 

u 

♦ 

«a. 

♦ 

»-4 

“>  I-. 

1 

«D  ro 

0. 

1 

1 

1 

1 ^ 

U) 

II 

w4  CVJ 

w 

— T 

U! 

«b  ^ 

♦ 

o 

♦ 

y- 

X 

X o . Ul 

♦ 

O’ 

1- 

« 

•a 

4 

4 

r> 

<t 

^ ►- 

♦ 

0. 

3 

» 

O 

It 

II  II  z 

O 

Q. 

< 

♦ 

a 

4 

*t 

•-< 

•H 

►H 

< 

Z 

O X 

♦ 

z 

4 

a. 

A 

^ ^ 

a 

M 

< 0^ 

♦ 

M 

4 

o 

II 

II 

IH 

“>  y z 

o 

IaJ  O 

« 

4 

O' 

O 

w 

— W O 

(X 

* » 

Of  u. 

* ♦ b 

» 

♦ 

b 

4 4 4 

a 

o 

^ X 

X 

X X o 

a 

00  115  1*1, 7, 3 


r 


N 


4 

4 

ro 

ro 

M 

l-t 

X 

T 

a 

a 

4 

4 

A 

in 

CO 

w 

< 

< 

4 

4 

<SJ 

CSJ 

♦4 

M 

M 

X 

r 

a 

a 

4 

4 

r^ 

< 

< 

w 

<VJ  ^ 
▼4  (T 


X 

a 


4- 

fO 

<SJ 

M 

X 

a 

4 


fO  M Oi 
W X 
< a «t 


rr 

w <r 
< 

4 < 

ro  « 
«-i  ro 
r-H  iNj 
T *-• 
Q X 

4 a 


w ^J 

II 

♦ 4 

M •-•  »-l 

II  H n 

n V a 


V 

k *1 

a a 

II  II 
I M n 

V V 

w ^ 

a.  0. 


fo 

K 


II 

«r 


o 

^J 


o 

o 


(M 

♦ 

< «J 
h-l  M 

K II 

cn  o 


4 

.4 

4 

4 

to 

ro 

<r 

,Pk, 

cr 

lO 

M 

w 

M 

«t 

w 

X 

X 

* 

a 

4 

a 

fv 

4 

4 

CM 

4 

A 

»-< 

XT' 

CM 

M 

UN 

X 

w 

w 

X 

w 

CL 

< 

CL 

«1 

4 

w 

* 

4 

4 

4 

CM 

CU 

ro 

ro 

fvj 

•» 

M 

« 

K 

X 

X 

X 

.IM. 

CL 

o 

* 

Q 

«t 

M 

M 

•0 

V 

V 

4 

4 

r^ 

4 

►H 

•k 

#» 

•k 

•k 

•k 

«k 

«k 

• 

«-k 

f*) 

<r 

fr 

o 

<t 

cr 

o 

CP 

o 

t 

►-I 

ir 

II 

»-♦ 

M 

M 

H' 

w 

T-* 

X 

w 

fO 

w 

w 

w 

w 

«2 

►-< 

«t 

a 

cr 

M 

a 

a 

a 

a 

a 

a 

a 

a 

Q 

II 

I 

II 

II 

11 

4 

4 

a 

#>k 

C» 

II 

II 

II 

II 

II 

II 

It 

II 

H 

hH 

4 

-) 

V 

-> 

fVJ 

M 

•k 

•k 

• 

#1^ 

«i« 

< 

«I 

<S 

r 

II 

II 

*VJ 

ro 

■t 

in 

iP  N- 

<o 

cn  »-i 

vC 

M 

cr 

►H 

M 

o 

w 

w 

W 

W 

^1* 

o 

~) 

V 

< 

< 

< 

< 

< 

4 

<i 

a 

< 

d.  «i 

a 

a 

in 


I o 


58 


P(I9,K) =PHI33* (A(8»+PHI23*At9) ) 
P(IC,K) =PHr33*PMr33*A (9) 

IP  ( lA  .FQ.  I ) GO  TO  123 
pr  Tq.TI  f ? 1 A f 4.0JTi  I 


r 


UPHATE  P,X 


p 


^ K 

p^ 

N 

^- 

c 

K 

N 

p^ 

A 

p^ 

p^ 

P- 

w 

w 

w 

W 

w 

W* 

< 

< 

<t 

< <X 

< 

<I 

4 

<t 

d 

* 

* 

» 

• 

#> 

* 

« 

* 

» 

* 

* 

4 



N. 

p- 

7 

7 

N- 

<o 

O' 

p- 

1^ 

p- 

p- 

K. 

«k 

•v 

•k 

•k 

•k 

•k 

•k 

•k 

«k 

•k 

•k 

•k 

•k 

rj 

fO 

lA 

vC 

K 

fw 

Ai 

fO 

J- 

iTi 

w 

w 

w 

w 

w 

a 

o 

a 

a 

a 

a 

Q 

a 

o 

o 

o 

a 

Q. 

0. 

♦ 

♦ 

♦ 

♦ 

♦ 

♦ 

♦ 

♦ 

♦ 

4* 

4- 

♦ 

4> 

A 

.♦ 

sj“ 

ktf- 

.♦ 

.4 

w 

w 

W 

W 

w 

w 

w 

Htr 

w 

«a 

<x 

«a 

« 

4. 

< 

d 

«X 

d 

d 

« 

» 

» 

* 

» 

* 

» 

♦ 

« 

» 

» 

* 

4 

4 

4 

UV  vO 

P- 

00  fA 

J' 

4- 

4- 

s 

IT 

» 

•v 

•v 

» 

•k 

*> 

•k 

» 

%# 

•k 

•k 

» 

•k 

•k 

»-•  «V«  ^J 

^■t 

.♦ 

.♦ 

»-4 

> 

«4 

Al 

m 

W 

W 

w 

W 

w 

w 

w* 

W> 

a. 

a 

a 

Q 

o 

a 

a 

a a 

> 

-j 

* 

Q. 

a 

a 

a 

a 

lij 

♦ 

♦ 

♦ 

♦ 

♦ 

4- 

4> 

♦ 

4- 

* 

o 

♦ 

4- 

♦ 

4 

M 

4 

#Mk 

-i 

«-l 

»-• 

< 

kS 

v4 

▼H 

W 

w 

w 

w 

<r 

z 

w 

w 

K-k 

o. 

< 

< 

< 

« 

«t 

< 

< 

<I 

<X 

< 

T. 

o 

HI 

1 

<1 

«x 

«t 

d 

O 

•-* 

* 

* 

» 

» 

» 

» 

* 

* 

« 

♦ 

M 

<r 

15 

« 

* 

4 

• 

4 

1 

(T 

"> 

«Pik 

ro 

» 

♦ 

•k 

A 

»-•  M M 

nS 

4i> 

4^ 

O' 

•k 

r 

V 

HI 

«4 

rj 

4 

ITi 

-1 

•k 

•v 

«% 

•k 

•k 

•k 

•k 

•k 

•k 

K 

I*' 

O' 

O' 

•k 

•k 

•k 

•k 

TH 

<r< 

rH 

•k 

>-i 

• 

•k 

Hi 

•k 

o 

T-1 

t4 

▼4 

O' 

fSi 

W 

w 

w 

w 

W 

IT 

w 

M 

W 

w 

w 

a 

a 

a 

a 

a 

a 

a 

0.  a. 

11 

Ul 

II 

> 

X 

II 

II 

Ul 

a 

a 

a 

u 

Q 

H 

II 

►H 

II 

rj 

II 

IH 

-0 

3 

II 

II 

M 

II 

II 

II 

II 

II 

II 

2 

II 

II 

-» 

II 

II 

II 

II 

II 

<t 

o 

H4 

< 

O 

o 

~> 

HI 

X 

l~ 

A 

A 

0^ 

A 

T 

2" 

CkJ 

A 

CM 

0^ 

o 

CVJ 

fn 

j’ 

lA  kO 

rr 

O' 

15 

o 

1-4 

M 

HI 

2 

rj 

fO 

4 

Uk 

M 

U1 

w 

w 

w 

o 

M 

o 

k-i  o 

w 

o 

w 

o 

W 

(/) 

o 

> 

> 

> 

> 

> 

> 

> 

> 

> 

c: 

to 

u 

V)  a 

o 

X 

o 

a 

o 

> 

> 

> 

> 

> 

w 

O O (Vi 


o o 

o (VJ  o 


61 


V(6)  = P(lf6»  ♦ A(l)  ♦ P(l»,6)  * A(4)  ♦ P(6,7)  ♦ ACT) 
V(7)  s PCI, 7)  ♦ ACl)  ♦ «>C4,7)  * AC4)  ♦ oC7,7)  • ACT) 


Appendix  C . 3 


Filter  Three : Modified  Potter  Filter  Program  1 

1 

Filter  three  is  a modification  of  the  Potter  algorithm. 

It  processes  the  measurement  vector  one  component  at  a time 
and  employs  the  covariance  update  technique  of 

(32) 

where  S(t^^)  is  calculated  by  Bierman's  FORTRAN  mechaniza- 
tion of  Potter's  data  processing  method  for  the  linear 
system  (Ref  2:31).  His  mechanization  is  modified  in  this 
program  for  the  nonlinear  system  and  is  improved  with 
respect  to  storage  requirements  and  execution  speed.  Propa- 
gation is  accomplished  by  the  Matrix  RSS  method  utilizing 
Equations  11  and  13  (Ref  7:Ch  7,  16.) 

This  program  is  written  in  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documentation 
consists  of  internal  comment  cards,  Fig.  7,  Bierman's  equa- 
tions and  FORTRAN  mechanization  (Ref  2:30-31). 
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Appendix  C . A 


Filter  Four:  Modified  U-D  Filter  Program 

Filter  four  Is  a modification  of  the  U-D  algorithm. 

It  processes  the  measurement  vector  one  component  at  a time 
and  employs  the  covariance  update  technique  of 

= U(t^'^)D(t^'^)U(t^‘^)'’^  (33) 

where  and  D(t^^)  are  calculated  by  Blerman's  FORTRAN 

mechanization  In  the  U-DCtj^"^)  form  for  the  linear  system 
(Ref  2:101).  His  mechanization  is  modified  in  this  program 
for  the  nonlinear  system  and  is  improved  with  respect  to 
storage  requirements  and  execution  speed.  Propagation  is 
accomplished  by  the  Matrix  RSS  method  utilizing  Equations 
11  and  13  (Ref  7:Ch  7,  16). 

This  program  is  written  in  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documentation 
consists  of  internal  comment  cards,  Fig.  8,  Bierman's  equa- 
tions (Ref  2:77-80)  and  FORTRAN  mechanization  (Ref  2:101). 
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P<I9,K)=PMI33*  (A  (8)4PHI21»AO)  ) 
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IF  ( lA  .E9.  I > GO  TO  120 
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IF  ( L .NF.  1 ) 60  TO  210 
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Appendix  C . 5 


Filter  Five : Modified  Upper  Triangular  Filter  Program 

Filter  five  is  a modification  of  the  upper  triangular 
algorithm.  It  processes  the  measurement  vector  one  compo- 
nent at  a time  and  employs  the  covariance  update  technique 
of 

= U(t^'^)U(t.'^)'^  (34) 

where  UCtj^"^)  is  calculated  by  Bierman's  FORTRAN  mechaniza- 
tion for  the  linear  system  (Ref  2:102).  His  mechanization 
is  modified  in  this  program  for  the  nonlinear  system  and  is 
improved  with  respect  to  storage  requirements  and  execution 
speed.  Propagation  is  accomplished  by  the  Matrix  RSS 
method  utilizing  Equations  11  and  13  (Ref  7:Ch  7,  16). 

This  program  is  written  in  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documentation 
consists  of  internal  comment  cards,  Fig.  9,  and  Bierman's 


equations  (Ref  2:81)  and  FORTRAN  mechanization  (Ref  2:102). 
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Appendix  C.6 


Filter  Six:  Standard  Kalman  Filter  Program 

Filter  six  represents  the  usual  method  of  Implementing 
the  extended  Kalman  filter  Into  a computer  program.  It 

processes  the  measurement  vector  all  at  once.  This  Is  I 

accomplished  through  the  use  of  general  purpose  subroutines  | 

that  waste  storage  and  require  additional  time  to  execute.  | 

The  program  employs  the  covariance  update  technique  of 

- P(t^")  - ffiCtjL")  (35) 

Since  this  program  Is  the  usual  method  of  Implementing  the 
extended  Kalman  filter,  It  serves  as  a worst  case  reference 
for  the  other  filters. 

This  program  Is  written  In  American  National  Standards 
Institute  (ANSI)  FORTRAN,  as  described  in  X. 3-1966  with  the 
exception  of  the  PROGRAM  instruction.  Program  documentation 
consists  of  internal  comment  cards.  Fig.  10,  and  the  usual 
Kalman  filter  equations  (Ref  6:65-66). 
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PROGRAM  eXM05(IMPUT,OUTPUT,TAPEl*IMPUT,TAPE2sOUTPUT) 
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C V(9,3),  H(9,9) ,V(9,9) ,G(9,3) ,2F(3,1),P(9,1) ,S ( 3,3) , T ( 3 , 3) ,U(3,9) 

real  DELTA,r>EN0MC,0EN0M9,3=HDMA,F,G,H,P,PHI,0,R,S,T,TA0,TEM3,*J,UA, 


» DELTA 

PMI(I,K)  » ( -TAU  ♦ DELTA  ♦ TAU  * TEMP  ) • TAU 

PHKJtJ)  = 1.0 

PHI(J,<)  = TAU  • (1.  - TEMP) 
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The  feasibility  of  adapting  an  extended  Kalman  filter  to  the 
jreal-time  satellite- to-satelllte  tracking  problem  is  investigated. 

|A  filter  using  linear  propagation  and  nonlinear  measurements  is 
formulated.  This  filter  accepts  tracker  measurements  to  provide  the] 
state  estimates  (position,  velocity,  and  acceleration)  of  the 
tracked  satellite  in  the  local  inertial  reference  frame. 

Six  different  filters  are  programmed  in  American  National 
Standards  Institute  (ANSI)  FORTRAN.  The.se  filters  employ  (cont) 
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11/45  minicomputer.  A final  filter  is  selected  as  best  for  the 
real-time  satellite-to-satellite  tracking  problem. 
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